#!/usr/bin/env roseus
(require "package://pr2eus/pr2-interface.l")
(ros::roseus "present-motion")
;;(ros::roseus-add-srvs "present_motion")

(defun prepare ()
  (unless (boundp '*pr2*) (pr2))
  (unless (boundp '*ri*)
    (setq *ri* (instance pr2-interface :init))))

(defun present-motion ()
  (send *pr2* :rarm :inverse-kinematics (make-coords :pos (float-vector 600 -200 1000)))
  (send *pr2* :head :look-at (send *pr2* :rarm :end-coords :worldpos))
  (send *ri* :angle-vector (send *pr2* :angle-vector))
  (send *ri* :wait-interpolation)
  (send *ri* :stop-grasp :rarm)
  (unix:sleep 5)
  (pr2-tuckarm-pose :rarm)
  (send *ri* :start-grasp :rarm)
  )

(defun pr2-grasp-choco-pose (&rest args)
  (pr2-tuckarm-pose)
  (let* ((current-arm (check-tuckarm-pose :thre 50)) ;; nil rarm larm
         (free-arm (or (car args) current-arm :larm))
		 (msec 2000)
         (av-lst
          (list
           (float-vector -50 0 -150 -80 0 0 0)
           (float-vector -117.421 -20 -220 -110 -3.75561 -50 149.611)
           (float-vector -117.421 -1.85502 -214.683 -121.53 -3.75561 -11.8644 149.611))))
    ;;
    (send *pr2* :larm :angle-vector #f(25 0 0 -130 0 0 0))
    (send *pr2* :rarm :angle-vector #f(-25 0 0 -130 0 0 0))
    (send *pr2* :torso :waist-z :joint-angle 51.4717)
    (send *ri* :angle-vector (send *pr2* :angle-vector) msec)
    (send *ri* :wait-interpolation)
    ;;
    (send *ri* :stop-grasp :rarm)
    ;;
    ;;(float-vector 51.4717 3.55173 61.3982 105.121 -82.2645 -99.3717 -5.72958 -4.93811 -117.421 -1.85502 -214.683 -121.53 -3.75561 -11.8644 149.611 34.5964 -21.2682))
    ;;
    (dolist (av av-lst)
      (send *pr2* :rarm :angle-vector av)
      (send *ri* :angle-vector (send *pr2* :angle-vector) msec)
      (send *ri* :wait-interpolation))
    ;;
    (send *ri* :start-grasp :rarm)
    (dolist (av (reverse av-lst))
      (send *pr2* :rarm :angle-vector av)
      (send *ri* :angle-vector (send *pr2* :angle-vector) msec)
      (send *ri* :wait-interpolation)))
  ;; (pr2-tuckarm-pose)
  )


(defun grasp-choco ()
  #f(51.4717 3.55173 61.3982 105.121 -82.2645 -99.3717 -5.72958 -4.93811 -117.421 -1.85502 -214.683 -121.53 -3.75561 -11.8644 149.611 34.5964 -21.2682))


(defun motion-cb (req)
  (present-motion)
  (let ((res (send req :response)))
    (send res :finish_present t)
    res))

;; (ros::advertise-service "present_motion" present_motion::PresentMotion #'motion-cb)

(defun ros-spin ()
  (do-until-key (ros::spin-once)))